#  该版本实现完整功能，区分转弯路径点分配转弯速度，直线段有稠密化，改进扫描路线覆盖存在留白问题，未解决可能出现扫描路径超出凹边界的问题，相比英文版修改了可视化部分，时间优化版，基于V7cn优化回退条带划分分水平竖直，多边形顶点如果不按顺序输入若改会自动形成凸包如果实际是非凸区域未改，画图优化
#  增加给定无人机数量对于目标区域是否合适判断

import numpy as np
from shapely.geometry import Polygon, MultiPolygon, LineString, MultiLineString, box, Point, MultiPoint
from shapely.ops import unary_union, voronoi_diagram # voronoi_diagram is new in Shapely 2.0
                                                   # For older Shapely, we'd use scipy.spatial.Voronoi + manual clipping
import matplotlib.pyplot as plt
import matplotlib.cm as cm
from scipy.optimize import linear_sum_assignment # For optimal UAV-to-region assignment
from sklearn.cluster import KMeans, MiniBatchKMeans # For k-means based area partitioning
import random, time # For sampling points
from typing import Tuple # For type hinting

class AreaCoveragePlanner:
    """无人机区域覆盖路径规划器
    
    功能：
    - 使用Voronoi图划分区域
    - 为每个无人机生成平滑的扫描路径
    - 区分直线段和转弯段分配不同速度
    
    主要参数：
    - uav_ids: 无人机ID列表
    - polygon_vertices: 目标区域顶点坐标
    - flight_altitude: 飞行高度(米)
    - sweep_width: 传感器扫描宽度(米)
    - scan_speed: 直线扫描速度(m/s)
    - turn_speed: 转弯速度(m/s)
    
    典型用法：
    1. 初始化规划器
    2. 调用plan_routes()生成路径
    3. 使用_plot_results()可视化结果
    """
    def __init__(self, uav_ids: list, polygon_vertices: list,
                 uav_current_positions: list, flight_altitude: float,
                 sweep_width: float, 
                 scan_speed: float, turn_speed: float,
                 turn_radius_factor: float = 0.4,
                 num_sampling_points_factor: int = 200,
                 path_boundary_standoff: float = 2, 
                 min_fallback_standoff: float = 0.01, 
                 max_segment_length: float = None,
                 # New parameters for suitability check:
                 uav_endurance_time_sec: float = 15*60,      # UAV endurance in seconds
                 uav_battery_reserve_factor: float = 0.2, # e.g., 0.2 for 20% reserve
                 uav_setup_time_sec: float = 5*60,    # e.g., 5 mins for pre/post flight activities
                 sensor_overlap_factor: float = 0.1, # e.g., 0.1 for 10% sensor overlap
                 random_seed= None  # Optional random seed for reproducibility
                 ):
        """初始化区域覆盖路径规划器
        
        参数：
        uav_ids: 无人机ID列表，如['UAV1', 'UAV2']
        polygon_vertices: 目标区域顶点坐标列表，每个顶点是[x,y,z]格式
        uav_current_positions: 无人机当前位置列表，每个位置是[x,y,z]格式  
        flight_altitude: 飞行高度(米)
        sweep_width: 传感器扫描宽度(米)
        scan_speed: 直线飞行时的扫描速度(m/s)
        turn_speed: 转弯时的飞行速度(m/s)
        turn_radius_factor: 转弯半径与扫描宽度的比例因子
        num_sampling_points_factor: 区域划分时的采样点数量因子
        path_boundary_standoff: 路径与边界的间隔距离(米)
        min_fallback_standoff: 最小备用间隔距离(米)
        max_segment_length: 路径最大分段长度(米)，None表示不限制
        
        重要说明：
        1. path_boundary_standoff应小于sweep_width/2，否则会产生覆盖间隙
        2. turn_speed通常应小于scan_speed以确保转弯安全
        3. 所有距离参数单位必须一致(建议使用米)
        """
        
        # Validation for suitability check parameters
        if uav_endurance_time_sec <= 0:
            raise ValueError("uav_endurance_time_sec must be positive.")
        if not (0 <= uav_battery_reserve_factor < 1):
            raise ValueError("uav_battery_reserve_factor must be between 0 (inclusive) and 1 (exclusive).")
        if uav_setup_time_sec < 0:
            raise ValueError("uav_setup_time_sec cannot be negative.")
        if not (0 <= sensor_overlap_factor < 1):
            raise ValueError("sensor_overlap_factor must be between 0 (inclusive) and 1 (exclusive).")
        
        self.uav_endurance_time_sec = uav_endurance_time_sec
        self.uav_battery_reserve_factor = uav_battery_reserve_factor
        self.uav_setup_time_sec = uav_setup_time_sec
        self.sensor_overlap_factor = sensor_overlap_factor
        # New attributes to store suitability check results
        self.last_suitability_check = {
            "is_suitable": None,  # Will be True, False, or None if check hasn't run
            "recommended_uav_count": None, # Stores the recommended number
            "message": ""         # Stores the human-readable message/judgment
        }
        
        self.sweep_width = sweep_width
        self.path_boundary_standoff = path_boundary_standoff
        self.min_fallback_standoff = max(1e-4, min_fallback_standoff) # Ensure it's a tiny positive
        if self.path_boundary_standoff < 0:
            print("Warning: path_boundary_standoff is negative. Setting to 0.0.")
            self.path_boundary_standoff = 0.0
        
        # Warning about potential coverage gaps (remains relevant)
        if self.path_boundary_standoff >= self.sweep_width / 2.0 and self.path_boundary_standoff > 0:
            print(f"Warning: path_boundary_standoff ({self.path_boundary_standoff:.2f}m) "
                  f"is >= sweep_width/2 ({self.sweep_width/2.0:.2f}m). "
                  "This may lead to coverage gaps at the boundary if not intended.")
        
        # ...
        # self.uniform_speed = uniform_speed # Remove this
        self.scan_speed = scan_speed
        self.turn_speed = turn_speed
        if turn_speed > scan_speed:
            print("Warning: turn_speed is greater than scan_speed. This is unusual.")
        self.max_segment_length = max_segment_length
        # # Minimum distance turn points should be from the true boundary
        # self.turn_standoff_distance = turn_standoff_factor * sweep_width 
        # # Ensure standoff is not negative if sweep_width is unexpectedly small or factor is negative
        # self.turn_standoff_distance = max(0.0, self.turn_standoff_distance) 
        # ... rest of __init__ ...
        self.uav_ids = uav_ids
        self.num_uavs = len(uav_ids)
        self.polygon_vertices_3d = np.array(polygon_vertices)
        self.polygon_vertices_2d = [(v[0], v[1]) for v in self.polygon_vertices_3d]
        self.uav_current_positions = [np.array(pos) for pos in uav_current_positions]
        self.flight_altitude = flight_altitude
        self.sweep_width = sweep_width
        self.turn_radius = turn_radius_factor * sweep_width
        self.num_sampling_points = self.num_uavs * num_sampling_points_factor

        self.main_polygon_shapely = None
        self.assigned_sub_polygons_to_uavs = [None] * self.num_uavs
        self.all_uav_routes_2d_for_plotting = {}

        self.random_seed = random_seed
    
    def _check_uav_suitability_and_suggest(self) -> Tuple[bool, int]:
        """
        检查当前无人机数量是否适合任务区域和续航能力。
        如果当前数量过少，则建议一个最小数量。
        此方法假定 _create_main_polygon() 已被调用。

        返回:
            Tuple[bool, int]: (is_suitable, recommended_uav_count)
        1.近似值： 这是一个 total_scan_length_estimate = total_area / effective_sweep_width 常见但粗略的近似值。它不考虑转弯期间的额外行驶距离或子区域可能效率低下的形状。实际飞行距离总是会稍长一些。您可以添加一个“低效因子”（例如，将 total_scan_length_estimate 乘以 1.1-1.3）来更保守
        2.average_speed_for_estimation： 使用 self.scan_speed 是乐观的。如果路径有很多转弯，实际平均速度会更低。如果你的路径非常重转弯，你可以使用 (self.scan_speed + self.turn_speed) / 2 甚至更保守的估计
        """
        # Initialize/reset suitability info for this check
        self.last_suitability_check = {
            "is_suitable": False,
            "recommended_uav_count": self.num_uavs, # Default to current if check fails early
            "message": ""
        }
        message_parts = [] # Collect parts of the message

        if self.main_polygon_shapely is None or self.main_polygon_shapely.is_empty:
            msg = "错误: 主区域未定义或为空，无法执行无人机数量适用性检查。"
            # print(msg)
            self.last_suitability_check["message"] = msg
            # is_suitable remains False, recommended_uav_count is current num_uavs
            return False, self.num_uavs

        total_area = self.main_polygon_shapely.area
        if total_area < 1e-3: # 区域面积过小
            msg = "提示: 目标区域面积过小，任何数量的无人机均视为合适。"
            # print(msg) # Optional print, or let the caller decide based on info
            self.last_suitability_check["is_suitable"] = True
            self.last_suitability_check["recommended_uav_count"] = max(1, self.num_uavs if self.num_uavs > 0 else 1)
            self.last_suitability_check["message"] = msg
            return True, self.last_suitability_check["recommended_uav_count"]

        # 考虑重叠后的有效扫描宽度
        effective_sweep_width = self.sweep_width * (1 - self.sensor_overlap_factor)
        if effective_sweep_width < 1e-3: # 有效扫描宽度过小
            msg = "错误: 有效扫描宽度过小。无法进行有效覆盖。"
            # print(msg)
            self.last_suitability_check["message"] = msg
            return False, self.num_uavs + 1 # 表示需要更多

        # 估算总扫描路径长度 (面积 / 有效扫描宽度)
        # 这是一个近似值，未考虑转弯等复杂情况。
        total_scan_length_estimate = total_area / effective_sweep_width

        # 计算单架无人机可用于纯扫描的飞行时间
        # 使用 scan_speed 进行估算，因为这是主要的地面覆盖速度。
        # 转弯速度较慢，实际覆盖时间会更长，所以这是乐观估计。
        # 如果转弯非常频繁，可以使用加权平均速度。
        average_speed_for_estimation = self.scan_speed
        time_for_scanning_per_uav_sec = self.uav_endurance_time_sec * (1 - self.uav_battery_reserve_factor) \
                                     - self.uav_setup_time_sec
        
        if time_for_scanning_per_uav_sec <= 0:
            msg = (f"错误: 无人机单次可用扫描时间不足或为负 ({time_for_scanning_per_uav_sec:.2f} 秒)。"
                   "请检查续航、电池余量和准备时间参数。")
            # 如果一架无人机连最基本的扫描都完成不了，理论上需要无数多架。
            # 推荐一个比当前数量大很多的数，或标记为错误。
            # print(msg)
            self.last_suitability_check["message"] = msg
            self.last_suitability_check["recommended_uav_count"] = self.num_uavs + 10 # 示意性地增加10架
            return False, self.last_suitability_check["recommended_uav_count"]

        # 单架无人机可扫描的最大距离
        max_scan_length_per_uav = time_for_scanning_per_uav_sec * average_speed_for_estimation
        if max_scan_length_per_uav <= 0: 
            msg = f"错误: 无人机单次可扫描距离不足或为负 ({max_scan_length_per_uav:.2f} 米)。"
            # print(msg)
            self.last_suitability_check["message"] = msg
            self.last_suitability_check["recommended_uav_count"] = self.num_uavs + 10
            return False, self.last_suitability_check["recommended_uav_count"]

        # 单架无人机可覆盖的最大面积
        max_area_per_uav = max_scan_length_per_uav * effective_sweep_width
        if max_area_per_uav <= 1e-3 : # 单架无人机无法覆盖有意义的面积
            msg = f"错误: 单架无人机可覆盖的最大面积过小 ({max_area_per_uav:.2f} 平方米)。"
            # print(msg)
            self.last_suitability_check["message"] = msg
            self.last_suitability_check["recommended_uav_count"] = self.num_uavs + 10
            return False, self.last_suitability_check["recommended_uav_count"]

        # 计算推荐的最小无人机数量 (向上取整)
        min_recommended_uavs = np.ceil(total_area / max_area_per_uav)
        min_recommended_uavs = int(max(1, min_recommended_uavs)) # 至少需要1架，并确保是整数

        is_suitable = self.num_uavs >= min_recommended_uavs
        self.last_suitability_check["is_suitable"] = is_suitable
        self.last_suitability_check["recommended_uav_count"] = min_recommended_uavs

        if not is_suitable:
            message_parts.append(f"判断: 当前无人机数量 ({self.num_uavs}) 可能不足以在单次任务中完成对目标区域的扫描。")
            message_parts.append(f"  估算总面积: {total_area:.2f} 平方米")
            message_parts.append(f"  单架无人机有效扫描时间: {time_for_scanning_per_uav_sec:.0f} 秒 (约 {(time_for_scanning_per_uav_sec/60):.1f} 分钟)")
            message_parts.append(f"  单架无人机估算可覆盖最大面积: {max_area_per_uav:.2f} 平方米")
            message_parts.append(f"  推荐最小无人机数量: {min_recommended_uavs} 架")
        else:
            message_parts.append(f"判断: 当前无人机数量 ({self.num_uavs}) 合适 (或可能过多)。")
            message_parts.append(f"  基于估算，推荐的最小数量为: {min_recommended_uavs} 架。")
        
        final_message = "\n".join(message_parts)
        # print("\n" + final_message) # Print for immediate feedback
        self.last_suitability_check["message"] = final_message

        return is_suitable, min_recommended_uavs
    
    def _densify_path(self, path_2d: list) -> list:
        """对路径进行稠密化处理，确保相邻点间距不超过max_segment_length
        
        参数：
        path_2d: 二维路径点列表，每个点是(x,y)元组
        
        返回：
        稠密化后的路径点列表
        
        算法说明：
        1. 检查输入有效性，如果不需要稠密化则直接返回原路径
        2. 遍历路径中的每一段：
           - 计算段长度
           - 如果超过max_segment_length，则插入中间点
        3. 过滤掉可能产生的重复点
        
        注意事项：
        - max_segment_length为None或<=0时，不进行稠密化
        - 会保留原始路径的所有点，只增加中间点
        - 最终点间距不超过max_segment_length
        """
        if not self.max_segment_length or self.max_segment_length <= 0 or not path_2d or len(path_2d) < 2:
            return path_2d

        densified_path = [path_2d[0]]
        for i in range(len(path_2d) - 1):
            p_start = np.array(path_2d[i])
            p_end = np.array(path_2d[i+1])
            segment_vec = p_end - p_start
            segment_len = np.linalg.norm(segment_vec)

            # 如果段长度超过限制，插入中间点
            if segment_len > self.max_segment_length:
                num_new_points = int(np.ceil(segment_len / self.max_segment_length)) - 1
                if num_new_points > 0:
                    for j in range(1, num_new_points + 1):
                        intermediate_point = p_start + segment_vec * (j / (num_new_points + 1))
                        densified_path.append(tuple(intermediate_point))
            
            densified_path.append(tuple(p_end)) # 添加原始段的终点
        
        # 过滤可能由非常短的原始段产生的连续重复点
        final_densified_path = [densified_path[0]] if densified_path else []
        if densified_path:
            for k in range(1, len(densified_path)):
                if np.linalg.norm(np.array(densified_path[k]) - np.array(final_densified_path[-1])) > 1e-5:
                    final_densified_path.append(densified_path[k])
        return final_densified_path
    
    def _calculate_heading_change_deg(self, p_prev_coords, p_curr_coords, p_next_coords) -> float:
        """计算航向变化角度(度)
        
        参数：
        p_prev_coords: 前一个点坐标
        p_curr_coords: 当前点坐标
        p_next_coords: 下一个点坐标
        
        返回：
        航向变化角度，范围0(直行)到180度(U型转弯)
        
        计算原理：
        1. 计算进入向量(p_prev到p_curr)和离开向量(p_curr到p_next)
        2. 计算两个向量的夹角
        3. 返回夹角的度数表示
        
        特殊情况处理：
        - 零向量视为180度转弯
        - 使用点积计算夹角，确保数值稳定性
        """
        vec1 = np.array(p_curr_coords[:2]) - np.array(p_prev_coords[:2]) # Vector for incoming segment
        vec2 = np.array(p_next_coords[:2]) - np.array(p_curr_coords[:2]) # Vector for outgoing segment

        norm_vec1 = np.linalg.norm(vec1)
        norm_vec2 = np.linalg.norm(vec2)

        if norm_vec1 < 1e-6 or norm_vec2 < 1e-6:
            return 0.0  # No change if segment length is zero or points are coincident

        unit_vec1 = vec1 / norm_vec1
        unit_vec2 = vec2 / norm_vec2

        dot_product = np.dot(unit_vec1, unit_vec2)
        # Ensure dot_product is within [-1, 1] for arccos due to potential floating point inaccuracies
        dot_product = np.clip(dot_product, -1.0, 1.0)
        
        angle_rad = np.arccos(dot_product)
        # angle_rad is the angle between the two heading vectors.
        # If heading is the same, angle_rad = 0. If opposite, angle_rad = pi (180 deg).
        # This directly represents the change in heading.
        return np.degrees(angle_rad)
    
    def _create_main_polygon(self):
        """创建并验证主多边形Shapely对象
        
        功能：
        1. 根据顶点坐标创建多边形
        2. 检查多边形有效性
        3. 尝试修复无效多边形
        4. 验证最终多边形
        
        异常：
        - 如果多边形无效且无法修复，抛出ValueError
        - 如果多边形创建失败，抛出ValueError
        
        注意事项：
        - 使用buffer(0)尝试修复自相交等无效多边形
        - 最终多边形必须满足is_valid和is_empty检查
        """
        try:
            # 从顶点创建多边形
            self.main_polygon_shapely = Polygon(self.polygon_vertices_2d)
            
            # 检查多边形有效性
            if not self.main_polygon_shapely.is_valid:
                # 尝试修复无效多边形
                self.main_polygon_shapely = self.main_polygon_shapely.buffer(0)
                
                # 再次验证修复后的多边形
                if not self.main_polygon_shapely.is_valid or self.main_polygon_shapely.is_empty:
                    raise ValueError("输入多边形无效且无法修复，或修复后为空多边形")
                    
        except Exception as e:
            raise ValueError(f"创建Shapely多边形失败: {e}")
    

    def _sample_points_in_polygon(self, polygon: Polygon, num_points: int) -> np.ndarray:
        """在多边形内均匀采样点
        
        参数：
        polygon: 目标多边形
        num_points: 需要采样的点数
        
        返回：
        采样点坐标数组，形状为(num_points, 2)
        
        算法说明：
        1. 计算多边形的边界框
        2. 在边界框内随机生成点
        3. 检查点是否在多边形内
        4. 动态调整批处理大小以提高效率
        
        特殊情况处理：
        - 退化多边形(面积很小)直接返回中心点
        - 采样效率低时自动调整批处理大小
        """
        points = []
        min_x, min_y, max_x, max_y = polygon.bounds
        while len(points) < num_points:
            # Adjust batch size based on how many more points are needed
            # to avoid excessive iterations if the polygon is small relative to its bounding box.
            batch_size = (num_points - len(points)) * 2 # Heuristic for efficiency
            if polygon.area > 0: # Estimate efficiency
                efficiency = polygon.area / ((max_x - min_x) * (max_y - min_y))
                if efficiency > 0 and efficiency < 1.0:
                    batch_size = int( (num_points-len(points)) / efficiency) +10
            batch_size = max(10, min(batch_size, num_points * 5))


            rand_x = np.random.uniform(min_x, max_x, batch_size)
            rand_y = np.random.uniform(min_y, max_y, batch_size)
            for i in range(batch_size):
                point = Point(rand_x[i], rand_y[i])
                if polygon.contains(point):
                    points.append([rand_x[i], rand_y[i]])
                    if len(points) == num_points:
                        break
            if max_x - min_x < 1e-6 or max_y - min_y < 1e-6: # Polygon is degenerate
                print("Warning: Polygon bounds are too small for effective sampling.")
                if len(points) < num_points and polygon.centroid.within(polygon): # add centroid if empty
                     for _ in range(num_points - len(points)): points.append([polygon.centroid.x, polygon.centroid.y])
                break

        return np.array(points)

    def _partition_area_kmeans_voronoi(self):
        t_start_total = time.time()

        if self.main_polygon_shapely is None or self.main_polygon_shapely.is_empty:
            raise ValueError("主多边形未初始化或为空")
        if self.num_uavs == 0: return []
        if self.num_uavs == 1:
            return [self.main_polygon_shapely] if self.main_polygon_shapely.area > 1e-9 else [None]

        # --- Fallback Strip Partitioning Function (Modified) ---
        def _do_strip_partitioning(polygon_to_split, num_strips, uav_id_prefix="StripFallback"):
            results = [None] * num_strips
            if polygon_to_split is None or polygon_to_split.is_empty or polygon_to_split.area < 1e-9:
                return results
            
            minx, miny, maxx, maxy = polygon_to_split.bounds
            total_width = maxx - minx
            total_height = maxy - miny

            if num_strips <= 0: return results

            # Decide strip direction based on aspect ratio
            if total_width >= total_height:
                # Vertical strips (split along X-axis)
                # print(f"调试 ({uav_id_prefix}): 使用垂直条带划分 (宽度: {total_width:.2f}, 高度: {total_height:.2f})")
                if total_width > 1e-6:
                    strip_width_val = total_width / num_strips
                    current_minx = minx
                    for i in range(num_strips):
                        strip_end_x = maxx if i == num_strips - 1 else current_minx + strip_width_val
                        strip_bbox = box(current_minx, miny, strip_end_x, maxy)
                        intersected = polygon_to_split.intersection(strip_bbox)
                        if not intersected.is_empty and intersected.area > 1e-9:
                            if isinstance(intersected, Polygon): results[i] = intersected
                            elif isinstance(intersected, MultiPolygon):
                                largest_comp = max(intersected.geoms, key=lambda p: p.area, default=None)
                                if largest_comp and largest_comp.area > 1e-9: results[i] = largest_comp
                        current_minx += strip_width_val
                elif isinstance(polygon_to_split, Polygon) and polygon_to_split.area > 1e-9: # No width, assign to first strip
                     results[0] = polygon_to_split
            else:
                # Horizontal strips (split along Y-axis)
                # print(f"调试 ({uav_id_prefix}): 使用水平条带划分 (宽度: {total_width:.2f}, 高度: {total_height:.2f})")
                if total_height > 1e-6:
                    strip_height_val = total_height / num_strips
                    current_miny = miny
                    for i in range(num_strips):
                        strip_end_y = maxy if i == num_strips - 1 else current_miny + strip_height_val
                        strip_bbox = box(minx, current_miny, maxx, strip_end_y)
                        intersected = polygon_to_split.intersection(strip_bbox)
                        if not intersected.is_empty and intersected.area > 1e-9:
                            if isinstance(intersected, Polygon): results[i] = intersected
                            elif isinstance(intersected, MultiPolygon):
                                largest_comp = max(intersected.geoms, key=lambda p: p.area, default=None)
                                if largest_comp and largest_comp.area > 1e-9: results[i] = largest_comp
                        current_miny += strip_height_val
                elif isinstance(polygon_to_split, Polygon) and polygon_to_split.area > 1e-9: # No height, assign to first strip
                     results[0] = polygon_to_split
            return results
        # --- End of Modified Fallback Strip Partitioning Function ---

        # ... (rest of the _partition_area_kmeans_voronoi method, including sampling, KMeans, Voronoi logic,
        #      and calls to _do_strip_partitioning for various fallback scenarios) ...
        # Example of how it's called (already in your code):
        # if sampled_points.shape[0] < self.num_uavs:
        #     fallback_regions = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs)
        #     # ...
        # if not sub_regions: # No valid regions from Voronoi
        #     final_sub_regions_list = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs)
        # except ImportError / Exception:
        #     fallback_regions = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs, "ExceptionFallback")


        # [THE REST OF YOUR _partition_area_kmeans_voronoi METHOD BELOW]
        # Ensure all calls to _do_strip_partitioning within this method
        # will now use the adaptive version defined above.
        t_sampling_start = time.time()
        _calculated_num_samples = self.num_sampling_points 
        actual_num_sampling = max(self.num_uavs * 20, min(_calculated_num_samples, 3000)) 

        sampled_points = self._sample_points_in_polygon(self.main_polygon_shapely, actual_num_sampling)
        t_sampling_end = time.time()
        # print(f"Point sampling (Target: {actual_num_sampling}, Actual: {sampled_points.shape[0]} points) took: {t_sampling_end - t_sampling_start:.3f}s")

        if sampled_points.shape[0] < self.num_uavs: 
            # print(f"警告: 只采样到{sampled_points.shape[0]}个点，不足以为{self.num_uavs}架无人机划分区域。"
            #       "将回退到简单条带划分。")
            fallback_regions = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs, "InsufficientSamplesFallback")
            t_end_total = time.time()
            # print(f"Partitioning (fallback after insufficient samples) total time: {t_end_total - t_start_total:.3f}s")
            return fallback_regions

        t_kmeans_start = time.time()
        k_means_n_init = 1 
        kmeans = KMeans(n_clusters=self.num_uavs, random_state=0, n_init=k_means_n_init, tol=1e-3).fit(sampled_points)
        # kmeans = MiniBatchKMeans(n_clusters=self.num_uavs, random_state=0, n_init=k_means_n_init, batch_size=100).fit(sampled_points)
        centroids = kmeans.cluster_centers_
        t_kmeans_end = time.time()
        # print(f"KMeans clustering (n_init={k_means_n_init}) took: {t_kmeans_end - t_kmeans_start:.3f}s")
        
        try:
            t_voronoi_start = time.time()
            multi_point_centroids = MultiPoint([Point(c[0], c[1]) for c in centroids])
            voronoi_envelope = self.main_polygon_shapely 
            voronoi_regions_geom_collection = voronoi_diagram(multi_point_centroids, envelope=voronoi_envelope)
            t_voronoi_diagram_end = time.time()
            # print(f"Voronoi diagram generation took: {t_voronoi_diagram_end - t_voronoi_start:.3f}s")

            sub_regions = [] 
            for region_geom in voronoi_regions_geom_collection.geoms:
                if not isinstance(region_geom, Polygon): continue 
                clipped_region = self.main_polygon_shapely.intersection(region_geom)
                if hasattr(self, '_get_largest_valid_polygon_component'): # Check if method exists
                    cleaned_region = self._get_largest_valid_polygon_component(
                        clipped_region, min_area=1e-6, 
                        uav_id_for_debug="VoronoiCell", attempt_description="Clipping" 
                    ) 
                    if cleaned_region:
                        sub_regions.append(cleaned_region)
                elif not clipped_region.is_empty: 
                     if isinstance(clipped_region, Polygon) and clipped_region.area > 1e-6:
                        sub_regions.append(clipped_region)
                     elif isinstance(clipped_region, MultiPolygon):
                        largest_part = max(clipped_region.geoms, key=lambda p: p.area, default=None)
                        if largest_part and isinstance(largest_part, Polygon) and largest_part.area > 1e-6:
                            sub_regions.append(largest_part)

            t_voronoi_clipping_end = time.time()
            # print(f"Voronoi cell clipping & processing ({len(sub_regions)} regions) took: {t_voronoi_clipping_end - t_voronoi_diagram_end:.3f}s")
            
            final_sub_regions_list = [] # Initialize as an empty list to hold actual regions
            if not sub_regions: 
                print("警告: Voronoi分区未生成有效区域。回退到简单条带划分。")
                final_sub_regions_list = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs, "NoVoronoiRegionsFallback")
            else:
                final_sub_regions_list = sub_regions # Pass the actual generated regions

            t_end_total = time.time()
            # print(f"Partitioning (Voronoi path) total time: {t_end_total - t_start_total:.3f}s")
            return final_sub_regions_list 

        except ImportError as ie:
            # print(f"Voronoi分区不可用 (ImportError: {ie}). 回退到简单条带划分。")
            fallback_regions = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs, "ImportErrorFallback")
            t_end_total = time.time()
            # print(f"Partitioning (ImportError fallback) total time: {t_end_total - t_start_total:.3f}s")
            return fallback_regions
        except Exception: # Catching generic Exception for other Voronoi related issues
            import traceback
            # print(f"Voronoi分区过程中出错: {traceback.format_exc()}. 回退到简单条带划分。")
            fallback_regions = _do_strip_partitioning(self.main_polygon_shapely, self.num_uavs, "ExceptionFallback")
            t_end_total = time.time()
            # print(f"Partitioning (Exception fallback) total time: {t_end_total - t_start_total:.3f}s")
            return fallback_regions

        
    def _assign_uavs_to_regions(self, unassigned_regions: list):
        """使用匈牙利算法基于距离将无人机分配到子区域
        
        参数：
        unassigned_regions: 待分配的子区域多边形列表
        
        功能：
        1. 构建无人机位置到区域中心点的成本矩阵
        2. 使用匈牙利算法找到最优分配方案
        3. 将分配结果存入self.assigned_sub_polygons_to_uavs
        
        算法说明：
        - 使用线性求和分配(linear_sum_assignment)实现匈牙利算法
        - 最小化无人机到区域中心点的总距离
        - 处理无人机数量与区域数量不匹配的情况
        
        异常处理：
        - 无有效区域时所有无人机分配为None
        - 处理无效距离和无穷大成本
        """
        valid_regions = [r for r in unassigned_regions if r is not None and r.area > 1e-6]
        num_valid_regions = len(valid_regions)

        if num_valid_regions == 0:
            print("没有有效区域可供分配")
            self.assigned_sub_polygons_to_uavs = [None] * self.num_uavs
            return

        # 初始化分配结果为None
        self.assigned_sub_polygons_to_uavs = [None] * self.num_uavs
        
        # 创建成本矩阵：行是无人机，列是区域
        # 如果无人机数量多于区域数量，部分无人机不会被分配
        # 如果区域数量多于无人机数量，部分区域不会被分配
        
        num_assignable_uavs = min(self.num_uavs, num_valid_regions)
        
        cost_matrix = np.full((self.num_uavs, num_valid_regions), np.inf)

        region_centroids = [r.centroid for r in valid_regions]

        for uav_idx in range(self.num_uavs):
            uav_pos = self.uav_current_positions[uav_idx][:2] # 使用2D位置
            for region_idx in range(num_valid_regions):
                region_centroid_pos = np.array([region_centroids[region_idx].x, region_centroids[region_idx].y])
                cost_matrix[uav_idx, region_idx] = np.linalg.norm(uav_pos - region_centroid_pos)

        # 应用匈牙利算法
        # linear_sum_assignment为方阵或矩形矩阵找到最优分配方案
        # 最小化总成本
        uav_indices, region_indices = linear_sum_assignment(cost_matrix)
        
        assigned_region_indices_set = set()
        for uav_idx, region_idx in zip(uav_indices, region_indices):
            if region_idx < num_valid_regions: # 确保region_idx对valid_regions有效
                 # 检查此无人机是否有合理的分配成本(非无穷大)
                if cost_matrix[uav_idx, region_idx] != np.inf :
                    self.assigned_sub_polygons_to_uavs[uav_idx] = valid_regions[region_idx]
                    assigned_region_indices_set.add(region_idx)
        
        # 对于未通过匈牙利算法分配的无人机(如果无人机数量>区域数量)，它们保持None
        # 如果有未分配的有效区域和未分配无人机的区域，可以进行第二轮分配(贪心算法)
        # 目前，此最优分配适用于min(num_uavs, num_valid_regions)配对
        
        # 检查是否所有无人机都尽可能得到了分配
        assigned_count = sum(1 for p in self.assigned_sub_polygons_to_uavs if p is not None)
        print(f"已为{assigned_count}架无人机分配区域")

    def _generate_boustrophedon_path_smooth_turns(self, polygon_shapely: Polygon, uav_id_for_debug: str = "UAV") -> list:
        """生成带平滑转弯的Boustrophedon覆盖路径
        
        参数：
        polygon_shapely: 需要生成路径的多边形(Shapely Polygon对象)
        uav_id_for_debug: 用于调试的无人机标识符
        
        返回：
        平滑的路径点列表，每个点是一个(x,y)坐标
        
        算法说明：
        1. 创建操作多边形(考虑边界偏移)
        2. 生成基本的Boustrophedon路径
        3. 在转弯处添加平滑过渡
        
        边界处理：
        - 使用path_boundary_standoff创建内部偏移多边形
        - 如果主偏移失败，使用min_fallback_standoff
        - 如果所有偏移都失败，使用原始多边形
        
        注意事项：
        - 确保路径点都在操作多边形内部
        - 处理空多边形或无效多边形情况
        - 平滑转弯半径不超过设定的turn_radius
        """
        if polygon_shapely is None or polygon_shapely.is_empty or polygon_shapely.area < 1e-6:
            return []

        operational_polygon = None
        current_standoff_used = self.path_boundary_standoff
        log_prefix = f"调试 ({uav_id_for_debug}, 区域面积: {polygon_shapely.area:.1f})"

        # 尝试1: 使用用户定义的path_boundary_standoff
        if self.path_boundary_standoff > 1e-5: # 仅在standoff有意义时进行缓冲
            temp_op_poly = polygon_shapely.buffer(-self.path_boundary_standoff)
            if not temp_op_poly.is_empty and temp_op_poly.is_valid and temp_op_poly.area > 1e-6:
                operational_polygon = temp_op_poly
                # print(f"{log_prefix}: 使用主standoff {self.path_boundary_standoff:.2f}m. 操作区域面积: {operational_polygon.area:.1f}")
            else:
                print(f"{log_prefix}: 主standoff {self.path_boundary_standoff:.2f}m 失败. 缓冲区域面积: {temp_op_poly.area if temp_op_poly else 'N/A'}.")
                current_standoff_used = self.min_fallback_standoff # 尝试备用方案
        else: # 用户希望路径在边界上或standoff可忽略
            # print(f"{log_prefix}: Standoff可忽略 ({self.path_boundary_standoff:.2f}m). 使用原始多边形.")
            operational_polygon = polygon_shapely
            current_standoff_used = 0.0


        # 尝试2 (备用方案): 如果主方案失败且standoff不是可忽略的，使用最小standoff
        if operational_polygon is None and current_standoff_used == self.min_fallback_standoff:
            temp_op_poly = polygon_shapely.buffer(-self.min_fallback_standoff)
            if not temp_op_poly.is_empty and temp_op_poly.is_valid and temp_op_poly.area > 1e-6:
                operational_polygon = temp_op_poly
                # print(f"{log_prefix}: 使用最小备用standoff {self.min_fallback_standoff:.2f}m. 操作区域面积: {operational_polygon.area:.1f}")
            else:
                print(f"{log_prefix}: 最小备用standoff {self.min_fallback_standoff:.2f}m 也失败. 缓冲区域面积: {temp_op_poly.area if temp_op_poly else 'N/A'}. "
                      "使用原始多边形. 路径将与边界重合.")
                operational_polygon = polygon_shapely
                current_standoff_used = 0.0
        elif operational_polygon is None and current_standoff_used == 0.0 : # Should have been caught by first 'else'
             operational_polygon = polygon_shapely


        # Final check if operational_polygon is usable
        if operational_polygon.is_empty or not operational_polygon.is_valid or operational_polygon.area < 1e-6:
            print(f"{log_prefix}: Final operational_polygon is unusable. No path will be generated.")
            return []

        # 所有后续几何计算都使用'operational_polygon'
        # 其边界与原始多边形边界保持current_standoff_used距离(除非current_standoff_used为0)
        minx, miny, maxx, maxy = operational_polygon.bounds
        
        # 计算多边形高度和宽度
        polygon_height = maxy - miny
        polygon_width = maxx - minx
        if polygon_height <= 1e-6 or polygon_width <= 1e-6: 
            return []  # 多边形太小无法生成路径

        # 初始化路径点列表
        basic_path_points = []
        y_curr = miny + self.sweep_width / 2.0  # 起始扫描线位置
        
        # 处理多边形高度小于扫描宽度的情况
        if polygon_height < self.sweep_width:
            y_curr = miny + polygon_height / 2.0
            y_curr = np.clip(y_curr, miny, maxy)  # 确保在边界内

        # 初始化扫描方向和计数器
        current_sweep_direction = 1  # 1表示从左到右，-1表示从右到左
        pass_count = 0
        max_passes = (2 * polygon_height / self.sweep_width) + 10  # 最大扫描次数限制

        # 生成基本Boustrophedon路径点
        while True:
            pass_count += 1
            if pass_count > max_passes: 
                break  # 防止无限循环
            
            # 计算当前扫描线边界
            current_sweep_lower_edge = y_curr - self.sweep_width / 2.0
            if current_sweep_lower_edge >= maxy + 1e-5: 
                break  # 扫描线超出多边形上边界

            # 创建扫描线并计算与多边形的交点
            scanline = LineString([
                (minx - self.sweep_width, np.clip(y_curr, miny, maxy)),
                (maxx + self.sweep_width, np.clip(y_curr, miny, maxy))
            ])
            intersection = operational_polygon.intersection(scanline)
            
            # 处理交点线段
            segments_on_this_line = []
            if not intersection.is_empty:
                # 收集所有线段几何体
                geoms_to_process = []
                if isinstance(intersection, LineString): 
                    geoms_to_process.append(intersection)
                elif isinstance(intersection, MultiLineString): 
                    geoms_to_process.extend(list(intersection.geoms))
                
                # 处理每个线段
                for line_segment in geoms_to_process:
                    if isinstance(line_segment, LineString):
                        coords = list(line_segment.coords)
                        if len(coords) == 2 and np.linalg.norm(np.array(coords[0]) - np.array(coords[1])) > 1e-5:
                            # 确保线段起点x坐标小于终点
                            if coords[0][0] > coords[1][0]: 
                                coords.reverse()
                            segments_on_this_line.append((coords[0], coords[1]))
                
                # 按x坐标排序线段
                segments_on_this_line.sort(key=lambda seg: seg[0][0])

            # 根据扫描方向添加路径点
            if segments_on_this_line:
                if current_sweep_direction == 1:
                    # 从左到右扫描：按顺序添加线段端点
                    for seg_start, seg_end in segments_on_this_line:
                        basic_path_points.append(seg_start)
                        basic_path_points.append(seg_end)
                else:
                    # 从右到左扫描：逆序添加线段端点
                    for seg_start, seg_end in reversed(segments_on_this_line):
                        basic_path_points.append(seg_end)
                        basic_path_points.append(seg_start)
                
                # 反转扫描方向
                current_sweep_direction *= -1

            # 如果多边形高度小于扫描宽度，只需一次扫描
            if polygon_height < self.sweep_width: 
                break
                
            # 移动到下一条扫描线
            y_curr += self.sweep_width
        if not basic_path_points: return []
        filtered_basic_path = [basic_path_points[0]]
        for i in range(1, len(basic_path_points)):
            if np.linalg.norm(np.array(basic_path_points[i]) - np.array(filtered_basic_path[-1])) > 1e-5:
                filtered_basic_path.append(basic_path_points[i])
        if len(filtered_basic_path) < 2: return filtered_basic_path
        
        smooth_path = [filtered_basic_path[0]] # Turn smoothing logic
        if len(filtered_basic_path) > 2:
            for i in range(1, len(filtered_basic_path) - 1):
                p0_basic = np.array(filtered_basic_path[i-1]); p1_basic = np.array(filtered_basic_path[i]); p2_basic = np.array(filtered_basic_path[i+1])
                v_in = p1_basic - p0_basic; v_out = p2_basic - p1_basic
                dist_in = np.linalg.norm(v_in); dist_out = np.linalg.norm(v_out)
                current_point_to_add = tuple(p1_basic); add_count = 0
                if dist_in > 1e-5 and dist_out > 1e-5:
                    v_in_norm = v_in / dist_in; v_out_norm = v_out / dist_out
                    dot_product = np.clip(np.dot(v_in_norm, v_out_norm), -1.0, 1.0)
                    if dot_product < 0.99:
                        max_rad_from_legs = min(dist_in / 2.01, dist_out / 2.01)
                        initial_attempt_radius = min(abs(self.turn_radius), max_rad_from_legs)
                        applied_turn_radius = initial_attempt_radius; final_t1, final_t2 = None, None
                        if applied_turn_radius > 1e-3:
                            for attempt in range(5):
                                t1_candidate = p1_basic - applied_turn_radius * v_in_norm; t2_candidate = p1_basic + applied_turn_radius * v_out_norm
                                point_t1 = Point(t1_candidate[0], t1_candidate[1]); point_t2 = Point(t2_candidate[0], t2_candidate[1])
                                if operational_polygon.contains(point_t1) and operational_polygon.contains(point_t2): # Check against operational_polygon
                                    final_t1, final_t2 = t1_candidate, t2_candidate; break
                                applied_turn_radius *= 0.7
                                if applied_turn_radius < 1e-3: break
                        if final_t1 is not None and final_t2 is not None:
                            current_point_to_add = [tuple(final_t1), tuple(final_t2)]; add_count = 2
                if add_count == 2:
                    if not smooth_path or np.linalg.norm(current_point_to_add[0] - np.array(smooth_path[-1])) > 1e-5: smooth_path.append(current_point_to_add[0])
                    if not smooth_path or np.linalg.norm(current_point_to_add[1] - np.array(smooth_path[-1])) > 1e-5: smooth_path.append(current_point_to_add[1])
                else:
                    if not smooth_path or np.linalg.norm(current_point_to_add - np.array(smooth_path[-1])) > 1e-5: smooth_path.append(current_point_to_add)
        if filtered_basic_path and (not smooth_path or np.linalg.norm(np.array(filtered_basic_path[-1]) - np.array(smooth_path[-1])) > 1e-5):
            smooth_path.append(filtered_basic_path[-1])
        # --- End of Turn Smoothing Logic ---
            
        return smooth_path


    def plan_routes(self) -> Tuple[dict, dict]:
        """规划所有无人机的覆盖路径
        
        返回：
        包含两个字典的元组：
        1. 无人机ID到3D路径点的映射
        2. 无人机ID到各路径段速度的映射
        
        算法步骤：
        1. 创建主多边形
        2. 无人机数量和续航适用性检查
        3. 划分区域并分配给无人机
        4. 为每个无人机生成平滑路径
        5. 计算各路径段的速度(区分直线段和转弯段)
        
        注意事项：
        - 处理无效或空区域情况
        - 路径稠密化处理
        - 优化路径起点选择
        """
        if self.random_seed is not None:
            np.random.seed(self.random_seed)

        self._create_main_polygon()

        # 执行无人机数量和续航适用性检查
        is_suitable, recommended_uavs = self._check_uav_suitability_and_suggest()

        if not is_suitable:
            print("提示: 由于无人机数量或续航能力判断为不合适，未执行路径规划。")
            # 根据用户要求，不合适则直接输出判断并返回空路径
            empty_routes = {uav_id: [] for uav_id in self.uav_ids}
            empty_speeds = {uav_id: [] for uav_id in self.uav_ids}
            return empty_routes, empty_speeds

        # 如果数量合适，继续执行现有的规划逻辑
        print("\n提示: 无人机数量判断为合适，开始进行路径规划...")

        unassigned_sub_regions = self._partition_area_kmeans_voronoi()
        self._assign_uavs_to_regions(unassigned_sub_regions)

        all_uav_routes_3d = {}
        all_uav_speeds_map = {} # 避免命名冲突
        self.all_uav_routes_2d_for_plotting = {}

        # 转弯判定阈值(度)
        # 航向变化超过此值视为转弯
        turn_angle_threshold_deg = 15.0 # 例如路径方向变化>15度视为转弯

        for i, uav_id in enumerate(self.uav_ids):
            sub_polygon = self.assigned_sub_polygons_to_uavs[i]
            
            # 处理无效或空区域情况
            if sub_polygon is None:
                all_uav_routes_3d[uav_id] = []
                all_uav_speeds_map[uav_id] = []
                self.all_uav_routes_2d_for_plotting[uav_id] = []
                continue

            # 生成平滑的Boustrophedon路径
            raw_path_2d = self._generate_boustrophedon_path_smooth_turns(sub_polygon, uav_id)

            # 如果需要，对路径进行稠密化处理
            if self.max_segment_length:
                raw_path_2d = self._densify_path(raw_path_2d) # 在长直线段上添加中间点
            
            # 处理无效路径情况
            if not raw_path_2d or len(raw_path_2d) < 2: # 至少需要2个点构成路径
                all_uav_routes_3d[uav_id] = []
                all_uav_speeds_map[uav_id] = []
                self.all_uav_routes_2d_for_plotting[uav_id] = [] 
                continue
            
            # 转换为3D路径(添加飞行高度)
            path_3d = [[p[0], p[1], self.flight_altitude] for p in raw_path_2d]

            # 优化路径起点选择
            current_pos_3d = self.uav_current_positions[i]
            np_current_pos = np.array(current_pos_3d[:3])
            np_path_start = np.array(path_3d[0][:3])
            np_path_end = np.array(path_3d[-1][:3])

            dist_to_path_start = np.linalg.norm(np_current_pos - np_path_start)
            dist_to_path_end = np.linalg.norm(np_current_pos - np_path_end)

            final_raw_path_2d_for_plotting = list(raw_path_2d) 
            if dist_to_path_end < dist_to_path_start and len(path_3d) > 1: # Check len > 1 for reverse
                path_3d.reverse()
                final_raw_path_2d_for_plotting.reverse()
            
            self.all_uav_routes_2d_for_plotting[uav_id] = final_raw_path_2d_for_plotting
            all_uav_routes_3d[uav_id] = path_3d
            
            # --- 速度分配逻辑 ---
            num_waypoints = len(path_3d)
            speeds = [self.scan_speed] * num_waypoints # 默认使用扫描速度

            if num_waypoints > 2:
                # 标记转弯点
                is_turn_waypoint = [False] * num_waypoints

                for k in range(1, num_waypoints - 1): # 评估路径点path_3d[k]
                    p_prev = path_3d[k-1]
                    p_curr = path_3d[k]
                    p_next = path_3d[k+1]
                    
                    heading_change_at_k_deg = self._calculate_heading_change_deg(p_prev, p_curr, p_next)
                    # 调试用：打印航向变化角度
                    # print(f"无人机 {uav_id}, 路径点 {k-1}-{k}-{k+1}: {k}点航向变化 {heading_change_at_k_deg:.1f} 度")

                    if heading_change_at_k_deg > turn_angle_threshold_deg:
                        is_turn_waypoint[k] = True # p_curr是转弯点

                # 根据转弯点标记分配速度
                # 路径点j的速度应用于段(j-1, j)
                for j in range(num_waypoints):
                    if j == 0: # 第一个路径点
                        # 如果路径直接从第一个点开始转弯
                        if num_waypoints > 1 and is_turn_waypoint[1]:
                            speeds[j] = self.turn_speed
                        else:
                            speeds[j] = self.scan_speed
                    else: # j > 0 的情况
                        # 如果当前点j或前一个点j-1是转弯点，
                        # 则段(j-1, j)属于机动段
                        if is_turn_waypoint[j] or is_turn_waypoint[j-1]:
                            speeds[j] = self.turn_speed
                        else:
                            speeds[j] = self.scan_speed
                
                # 特殊情况处理：转弯点后紧接着另一个转弯的短线段
                # 也应使用转弯速度。上述逻辑(检查当前和前一个点)已处理此情况
                # 例如倒角中的T1和T2点
                # Path: ..., P_scan_end, T1, T2, P_scan_start, ...
                # - At T1: is_turn_waypoint[T1_idx] = True.
                # - At T2: is_turn_waypoint[T2_idx] = True.
                # Speed for P_scan_end -> T1 (speeds[T1_idx]): is_turn_waypoint[T1_idx] is True OR is_turn_waypoint[P_scan_end_idx] is True.
                #   Since T1 is a turn, speeds[T1_idx] = turn_speed.
                # Speed for T1 -> T2 (speeds[T2_idx]): is_turn_waypoint[T2_idx] is True OR is_turn_waypoint[T1_idx] is True.
                #   Since T1 and T2 are turns, speeds[T2_idx] = turn_speed.

            elif num_waypoints == 2 : # Single segment
                speeds = [self.scan_speed, self.scan_speed]
            elif num_waypoints == 1:
                speeds = [self.scan_speed]


            all_uav_speeds_map[uav_id] = speeds
            
        return all_uav_routes_3d, all_uav_speeds_map

    # 辅助函数(可作为类外部函数或静态方法)
    # def _calculate_angle_at_waypoint(self, p_prev, p_curr, p_next):
    #     v1 = np.array(p_curr) - np.array(p_prev)  # 进入向量
    #     v2 = np.array(p_next) - np.array(p_curr)  # 离开向量
    #     # ... (实现代码块) ...
    #     dot_product = np.dot(v1, v2)  # 向量点积
    #     mag_v1 = np.linalg.norm(v1)   # 向量v1的模
    #     mag_v2 = np.linalg.norm(v2)   # 向量v2的模
        
    #     if mag_v1 < 1e-6 or mag_v2 < 1e-6: return 180.0  # 零向量视为180度转弯

    #     cos_angle = dot_product / (mag_v1 * mag_v2)  # 计算夹角余弦
    #     angle_rad = np.arccos(np.clip(cos_angle, -1.0, 1.0))  # 计算弧度值
    #     return np.degrees(angle_rad)  # 返回角度值
    
    def _plot_results(self, show_main_polygon=True, show_sub_regions=True, show_uav_starts=True):
        """可视化无人机区域划分和路径规划结果
        
        功能说明：
        1. 显示目标区域边界（蓝色线条）
        2. 用不同颜色显示各无人机分配的子区域（半透明填充）
        3. 绘制每架无人机的覆盖路径（带标记点的线条）
        4. 标记无人机初始位置（星形标记）
        
        参数说明：
        show_main_polygon - 是否显示主多边形边界
        show_sub_regions - 是否显示子区域划分
        show_uav_starts - 是否显示无人机起始位置
        
        绘图说明：
        1. 区域划分和路径规划结果在同一张图中显示
        2. 每个无人机的区域、路径和起点使用相同颜色
        3. 图例说明各元素的含义
        """
        """可视化显示区域划分和路径规划结果
        
        参数：
        show_main_polygon: bool, 是否显示主多边形边界(默认True)
        show_sub_regions: bool, 是否显示子区域划分(默认True) 
        show_uav_starts: bool, 是否显示无人机起始位置(默认True)

        功能描述：
        1. 创建Matplotlib图表，设置等比例坐标轴
        2. 根据参数绘制主多边形边界(黑色实线)
        3. 为每个无人机绘制分配的子区域(半透明填充)
        4. 绘制无人机飞行路径(带标记点)
        5. 标记无人机起始位置(特殊标记)
        6. 添加图例、标签和标题

        绘图细节：
        - 使用不同颜色区分不同无人机
        - 子区域使用半透明填充便于观察重叠
        - 路径使用实线连接，关键点有标记
        - 自动调整图例位置避免重叠

        注意事项：
        - 需要Matplotlib库支持
        - 在多无人机情况下会自动选择合适颜色
        - 图表尺寸固定为12x12英寸
        - 会阻塞程序执行直到关闭图表窗口
        """
        """Visualizes the coverage planning results with matplotlib."""
        # Initialize plot
        try:
            fig, ax = plt.subplots(figsize=(12, 12))
            ax.set_aspect('equal', adjustable='box')
        except Exception as e:
            print(f"Failed to initialize plot: {e}")
            return

        # # Plot main polygon
        # if show_main_polygon and self.main_polygon_shapely and not self.main_polygon_shapely.is_empty:
        #     try:
        #         x, y = self.main_polygon_shapely.exterior.xy
        #         ax.plot(x, y, color='blue', linewidth=1.5, solid_capstyle='round', label='Target Area')
        #     except Exception as e:
        #         print(f"Error plotting main polygon: {e}")

        # # Plot sub-regions
        # if show_sub_regions and self.num_uavs > 0 and self.assigned_sub_polygons_to_uavs:
        #     try:
        #         # Get colormap with fallback
        #         try:
        #             color_map = plt.colormaps['viridis'].resample(self.num_uavs if self.num_uavs > 1 else 2)
        #         except:
        #             color_map = plt.cm.viridis

        #         for k, sub_poly in enumerate(self.assigned_sub_polygons_to_uavs):
        #             if sub_poly is None or sub_poly.is_empty:
        #                 continue
                    
        #             if isinstance(sub_poly, Polygon):
        #                 x_poly, y_poly = sub_poly.exterior.xy
        #                 color = color_map(k % (self.num_uavs if self.num_uavs > 0 else 1))
        #                 label_text = f'UAV {self.uav_ids[k]} Region' if self.num_uavs > 1 else 'Target Region'
        #                 ax.fill(x_poly, y_poly, color=color, alpha=0.4, label=label_text if k==0 else None)
        #             elif isinstance(sub_poly, MultiPolygon):
        #                 for geom in sub_poly.geoms:
        #                     x_poly, y_poly = geom.exterior.xy
        #                     ax.fill(x_poly, y_poly, color=color, alpha=0.4)
        #     except Exception as e:
        #         print(f"Error plotting sub-regions: {e}")
        #     try:
        #         color_map = plt.colormaps['viridis'].resample(self.num_uavs if self.num_uavs > 1 else 2)
        #     except:
        #         color_map = plt.cm.viridis
            
        #     for i in range(self.num_uavs): # Iterate through UAVs
        #         sub_poly = self.assigned_sub_polygons_to_uavs[i] # Get polygon assigned to this UAV
        #         uav_id = self.uav_ids[i]
        #         if sub_poly and not sub_poly.is_empty:
        #             color = color_map(i / self.num_uavs if self.num_uavs > 1 else 0.5)
        #             label_text = f'{uav_id} Region'
        #             # ... (rest of polygon plotting logic for sub_poly) ...
        #             polygons_to_plot = []
        #             if isinstance(sub_poly, Polygon): polygons_to_plot.append(sub_poly)
        #             elif isinstance(sub_poly, MultiPolygon): polygons_to_plot.extend(list(sub_poly.geoms))
                    
        #             for k, poly_geom in enumerate(polygons_to_plot):
        #                 if isinstance(poly_geom, Polygon):
        #                     x_poly, y_poly = poly_geom.exterior.xy
        #                     ax.fill(x_poly, y_poly, color=color, alpha=0.4, label=label_text if k==0 else None)
        #                     ax.plot(x_poly, y_poly, color=color, linewidth=1, alpha=0.7)
        # # Plot UAV paths
        # if hasattr(self, 'all_uav_routes_2d_for_plotting') and self.all_uav_routes_2d_for_plotting:
        #     try:
        #         # Get path colormap with fallback
        #         path_cmap_name = 'cool' if self.num_uavs > 10 else 'winter'
        #         try:
        #             path_colors = plt.colormaps[path_cmap_name].resample(self.num_uavs if self.num_uavs > 0 else 1)
        #         except:
        #             path_colors = plt.cm.winter

        #         for i, uav_id in enumerate(self.uav_ids):
        #             if uav_id in self.all_uav_routes_2d_for_plotting and self.all_uav_routes_2d_for_plotting[uav_id]:
        #                 route = self.all_uav_routes_2d_for_plotting[uav_id]
        #                 x, y = zip(*route)
        #                 ax.plot(x, y, linestyle='--', marker='.', linewidth=1, 
        #                         color=path_colors(i), label=f'{uav_id} Path')
        #     except Exception as e:
        #         print(f"Error plotting UAV paths: {e}")

        # # Plot UAV starting positions
        # if show_uav_starts and self.uav_current_positions:
        #     try:
        #         for i, pos in enumerate(self.uav_current_positions):
        #             ax.plot(pos[0], pos[1], 'ro', markersize=8, 
        #                     label=f'UAV {self.uav_ids[i]} Start' if self.num_uavs > 1 else 'UAV Start')
        #     except Exception as e:
        #         print(f"Error plotting UAV starts: {e}")

        # # Final plot decorations
        # try:
        #     ax.set_xlabel("X-coordinate (meters)")
        #     ax.set_ylabel("Y-coordinate (meters)")
        #     ax.set_title("Area Partitioning and Coverage Paths")
        #     if any(line.get_label() != '_nolegend_' for line in ax.lines):  # Only show legend if we have items
        #         ax.legend()
        #     ax.grid(True)
        #     plt.show()
        #     fig, ax = plt.subplots(figsize=(12, 12))
        #     ax.set_aspect('equal', adjustable='box')
        # except Exception as e:
        #     print(f"Failed to initialize plot: {e}")
        #     return

        if show_main_polygon and self.main_polygon_shapely and not self.main_polygon_shapely.is_empty:
            x, y = self.main_polygon_shapely.exterior.xy
            ax.plot(x, y, color='black', linewidth=2, linestyle='-', solid_capstyle='round', label='Target Area Boundary')

        if show_sub_regions and self.num_uavs > 0:
            # 使用更鲜明的颜色方案
            region_cmap = plt.cm.tab20  # 20种高对比度颜色
            path_cmap = plt.cm.tab20b   # 另一组20种高对比度颜色
        
        for i in range(self.num_uavs):
            sub_poly = self.assigned_sub_polygons_to_uavs[i]
            uav_id = self.uav_ids[i]
            if sub_poly and not sub_poly.is_empty:
                # 使用相同的颜色索引确保区域和起点颜色一致
                color_val = region_cmap(i % region_cmap.N)
                label_text = f'{uav_id} Region'
                
                polygons_to_plot = []
                if isinstance(sub_poly, Polygon): polygons_to_plot.append(sub_poly)
                elif isinstance(sub_poly, MultiPolygon): polygons_to_plot.extend(list(sub_poly.geoms))
                
                for k, poly_geom in enumerate(polygons_to_plot):
                    if isinstance(poly_geom, Polygon):
                        x_poly, y_poly = poly_geom.exterior.xy
                        # 区域填充使用更饱和的颜色
                        ax.fill(x_poly, y_poly, color=color_val, alpha=0.5, label=label_text if k==0 else None)
                        ax.plot(x_poly, y_poly, color=color_val, linewidth=1.5, alpha=0.8)

        # 绘制路径和起点
        for i, uav_id in enumerate(self.uav_ids):
            route_2d = self.all_uav_routes_2d_for_plotting.get(uav_id)
            if route_2d and len(route_2d) > 1:
                x_path, y_path = zip(*route_2d)
                # 路径使用与区域同色系但更深的颜色
                path_color_val = region_cmap(i % region_cmap.N)
                ax.plot(x_path, y_path, linestyle='--', marker='.', 
                       linewidth=1, color=path_color_val, label=f'{uav_id} Path')
                # 起点使用与区域相同的颜色
                ax.plot(x_path[0], y_path[0], marker='*', markersize=12, 
                       color=path_color_val, markeredgecolor='black')

        # 绘制初始位置（与分配区域同色）
        if show_uav_starts and self.uav_current_positions:
            for i, uav_id in enumerate(self.uav_ids):
                pos = self.uav_current_positions[i]
                color_val = region_cmap(i % region_cmap.N)
                ax.plot(pos[0], pos[1], 'P', markersize=12, 
                       color=color_val, markeredgecolor='black', 
                       label=f'{uav_id} Start Pos')
        
        ax.set_xlabel("X-coordinate (meters)")
        ax.set_ylabel("Y-coordinate (meters)")
        ax.set_title(f"UAV Area Partitioning & Coverage Scan Paths ({self.num_uavs} UAVs)")
        
        handles, labels = ax.get_legend_handles_labels()
        # Remove duplicate labels for regions if any
        by_label = dict(zip(labels, handles))
        if len(by_label) > 10: # If many legend items, place outside
            ax.legend(by_label.values(), by_label.keys(), loc='upper center', bbox_to_anchor=(0.5, -0.05), fancybox=True, shadow=True, ncol=min(5, len(by_label)))
        else:
            ax.legend(by_label.values(), by_label.keys())
            
        ax.grid(True, linestyle=':', alpha=0.6)
        plt.tight_layout(rect=[0, 0.03, 1, 0.95]) # Adjust for title and legend
        plt.show()
    
if __name__ == '__main__':
    # ... (uav_ids_list, polygon_corners, uav_positions setup)
    num_test_uavs = 6# For example
    uav_ids_list = [f"UAV{j+1}" for j in range(num_test_uavs)] 
    # uav_ids_list = [4,2,8,9]  #允许两种ID格式： a) 带前缀：["UAV1", "UAV2"] b) 纯数字/字符：["1", "2"] 或 [1, 2]
    polygon_corners = [ [-197.74, -129.92,0], [-263.02, 43.11,0], [-425.16, -24.36,0], [-358.79, -187.59,0] ]
    # polygon_corners = [ [0,0,0], [900,0,0], [900,900,0], [0,900,0] ]
    # polygon_corners = [
    #      [0,0,0], [80,10,0], [120,60,0],[100,120,0], [70,80,0], [20,90,0], [-10,40,0]
    # ]
    # 梯形示例 (等腰梯形)
    # polygon_corners = [
    #     [0,0,0], [100,0,0], [80,80,0], [20,80,0]
    # ]
    # 五边形示例 (任意凸五边形)
    # polygon_corners = [
    #     [0,0,0], [80,20,0], [100,70,0], [50,100,0], [10,50,0]
    # ]
    # # 六边形示例 (任意凸六边形)
    # polygon_corners = [
    #     [0,40,0], [30,0,0], [80,0,0], [110,40,0], [80,80,0], [30,80,0]
    # ]
   
    # 生成多边形区域外的随机初始位置
    def generate_point_outside_polygon(polygon_corners):
        """在多边形区域外生成随机点"""
        # 计算边界框
        x_coords = [p[0] for p in polygon_corners]
        y_coords = [p[1] for p in polygon_corners]
        min_x, max_x = min(x_coords), max(x_coords)
        min_y, max_y = min(y_coords), max(y_coords)
        width = max_x - min_x
        height = max_y - min_y
        
        # 创建多边形对象用于包含检查
        polygon = Polygon([(p[0], p[1]) for p in polygon_corners])
        
        np.random.seed(42)
        while True:
            # 在扩展后的边界框外生成随机点
            margin = max(width, height) * 0.5  # 边界框外扩50%
            side = random.choice(['left','right','top','bottom'])
            
            if side == 'left':
                x = min_x - margin * (0.5 + random.random())
                y = min_y + random.random() * (height + 2*margin) - margin
            elif side == 'right':
                x = max_x + margin * (0.5 + random.random())
                y = min_y + random.random() * (height + 2*margin) - margin
            elif side == 'top':
                x = min_x + random.random() * (width + 2*margin) - margin
                y = max_y + margin * (0.5 + random.random())
            else: # bottom
                x = min_x + random.random() * (width + 2*margin) - margin
                y = min_y - margin * (0.5 + random.random())
            
            point = Point(x, y)
            if not polygon.contains(point):
                return [x, y, 15]  # 固定高度为15米

    uav_positions = []
    # for j in range(num_test_uavs):
    #     angle = 2 * np.pi * j / num_test_uavs
    #     radius = max(polygon_corners[1][0], polygon_corners[2][1]) * 0.3 
    #     uav_positions.append([np.cos(angle) * radius - 20, np.sin(angle) * radius - 20, 5])
    for _ in range(num_test_uavs):
        uav_positions.append(generate_point_outside_polygon(polygon_corners))


    target_flight_altitude = 15.0
    sensor_sweep_width = 20.0 # 生成扫描路径时，相邻两条平行扫描线的中心线之间的间隔直接使用 self.sweep_width，如果考虑重叠可设置比实际值小，再对应uav_sensor_overlap参数
    
    # New speed parameters
    uav_scan_speed = 5.0  # m/s for straight scanning parts
    uav_turn_speed = 4.0  # m/s for negotiating turns
    uav_max_segment_length = 20.0 # m, max length of straight segments

    uav_turn_radius_factor = 0.4 # How much of sweep_width is used for chamfer offset
    uav_sampling_points_factor = 50
    uav_path_boundary_standoff = 10.0 # m, standoff from polygon boundary

     # --- NEW PARAMETERS FOR SUITABILITY CHECK ---
    uav_flight_duration_sec = 15 * 60   # Example: 15 minutes  无人机续航时间，指单块电池或一次充满电后，一架无人机从起飞到因电量耗尽必须降落的总飞行时长（以秒为单位）
    uav_reserve_batt_factor = 0.20    # 20% battery reserve  无人机电池余量系数
    uav_flight_setup_time_sec = 2 * 60  # 2 minutes for takeoff/landing/travel to zone  无人机单次任务准备/结束时间，指的是无人机在执行扫描任务之外的“固定”时间开销（以秒为单位）
    uav_sensor_overlap = 0.0          # 10% overlap between scan lines 这个参数表示在相邻扫描条带之间，希望传感器扫描范围有多少比例的重叠。例如，设置为 0.1 表示希望相邻两次扫描的覆盖区域有10%的重叠

    try:
        planner = AreaCoveragePlanner(
            uav_ids=uav_ids_list,
            polygon_vertices=polygon_corners,
            uav_current_positions=uav_positions,
            flight_altitude=target_flight_altitude,
            sweep_width=sensor_sweep_width,
            scan_speed=uav_scan_speed,       # Pass new speed
            turn_speed=uav_turn_speed,       # Pass new speed
            turn_radius_factor=uav_turn_radius_factor,
            num_sampling_points_factor=uav_sampling_points_factor,
            path_boundary_standoff=uav_path_boundary_standoff,
            max_segment_length=uav_max_segment_length,
            # New endurance parameters
            uav_endurance_time_sec=uav_flight_duration_sec,
            uav_battery_reserve_factor=uav_reserve_batt_factor,
            uav_setup_time_sec=uav_flight_setup_time_sec,
            sensor_overlap_factor=uav_sensor_overlap,
            random_seed=42  # Optional, for reproducibility
        )
        # ... rest of the try-except block and printing ...
        t0 = time.time()
        planned_routes, planned_speeds = planner.plan_routes()
        t1 = time.time()

        # Access suitability information
        suitability_info = planner.last_suitability_check
        
        if not suitability_info["is_suitable"]:
            print("\n--- 无人机数量适用性检查结果 ---")
            print(f"  是否合适: 否")
            print(f"  推荐无人机数量: {suitability_info['recommended_uav_count']}")
            print(f"  详细信息: {suitability_info['message']}") # This message is already quite detailed
            # The plan_routes function would have already printed its own messages too.
        else:
            # Suitability check passed, routes should have been planned
            print("\n--- 无人机数量适用性检查结果 ---")
            print(f"  是否合适: 是")
            print(f"  (估算)推荐无人机数量: {suitability_info['recommended_uav_count']}")
            
            if not any(planned_routes.values()): # Should not happen if suitable, but good check
                print("\n路径规划似乎已执行但未生成有效路径，请检查日志。")
            else:
                print("\n--- Planned Routes and Speeds ---") 
                for uav_id in planned_routes:
                    route_points = planned_routes[uav_id]
                    speeds_at_waypoints = planned_speeds[uav_id] # Renamed for clarity
                    if not route_points:
                        print(f"\n无人机ID: {uav_id} - 未生成路径。")
                        continue
                    
                    print(f"\n无人机ID: {uav_id}")
                    print(f"  路径点 ({len(route_points)}):")
                    # Assuming speed at waypoint i is for segment ending at i
                    if route_points: # Print initial approach speed to first waypoint
                        print(f"    到WP0的进场速度: Speed {speeds_at_waypoints[0] if speeds_at_waypoints else 'N/A'}")

                    for idx, point in enumerate(route_points):
                        speed = speeds_at_waypoints[idx] if idx < len(speeds_at_waypoints) else "N/A"
                        print(f"    WP{idx}: ({point[0]:.2f}, {point[1]:.2f}, {point[2]:.2f}) @ {speed if isinstance(speed, str) else speed:.2f} m/s")
        
        print(f"\n规划总耗时: {t1-t0:.2f} 秒") # Total time including check + planning
        
        # Only plot if routes were actually generated
        if suitability_info["is_suitable"] and any(planned_routes.values()):
            planner._plot_results(show_sub_regions=True)

    except ValueError as ve:
        print(f"Error during planning: {ve}")
    except ImportError as ie:
        print(f"Import Error: {ie}. Please ensure all libraries are installed.")
    except Exception as e:
        print(f"An unexpected error occurred: {e}")
        import traceback
        traceback.print_exc()